;;;; -*- Mode: Lisp; Package: :dreaming-tree -*-
;;;;
;;;; controller.lisp
;;;;

(in-package :app.dt)

(defmethod attach ((obj controller) (s spatial))
  (with-slots (controllers) s
      (when controllers
	(push obj controllers))
      (setf controllers (list obj))))

(defmethod update-controllers ((controllers t) (spatial t) dt app-time)
  (declare (ignore app-time))
  )

(defmethod update-controllers ((controllers list) spatial dt app-time)
  (dolist (c controllers)
    (update-controllers c spatial dt app-time)))

(defmethod update-controllers ((cont spin-controller) (s spatial) dt app-time)
  (declare (ignore app-time))
  (with-slots (rx ry rz) cont
    (rotate-by s :rx rx :ry ry :rz rz)))

;;; Showcasing

(defclass random-thrust (controller)
  ((max-thrust :accessor max-thrust :initarg :max-thrust :initform 1.0)
   (last-update :accessor last-update :initform 0.0)))

(defclass thrust (force)
  ((direction :accessor direction :initarg :direction)
   (magnitude :accessor magnitude :initarg :magnitude)))

(defmethod apply-force ((f thrust) dt pos o lm am om lv av)
  (declare (ignore dt pos o lm am om lv av))
  (m* (direction f) (magnitude f)))

(defmethod update-controllers ((cont random-thrust) (rb rigid-body) dt app-time)
  (if (> (last-update cont) 2.0)
      (setf (force rb) (make-instance 'thrust :magnitude (random (max-thrust cont))
				      :direction (normalize (vector (random 1.0) (random 1.0) (random 1.0))))
	    (last-update cont) 0.0)
      (incf (last-update cont) (cdr app-time))))

(defclass move-to-control (controller)
  ((max-thrust :accessor max-thrust :initarg :max-thrust :initform 1.0)
   (max-velocity :accessor max-velocity :initarg :max-velocity :initform 2.0)
   (k :accessor k :initarg :k :initform 0.2)
   (target :accessor target :initarg :target :initform #(10.0 10.0 0.0))))

(defmethod update-controllers ((c move-to-control) (rb rigid-body) dt app-time)
  (with-slots (max-thrust max-velocity k target) c
    (let* ((pos (mass-center rb))
	   (err (m- target pos))
	   (target-vel (m* k err))
	   (verr (m- target-vel (linear-velocity rb))))
      (setf (force rb) (make-instance 'thrust :magnitude (* k (magnitude verr))
				      :direction (normalize verr))))))


;;; PID Loop Controller

(defclass pid-controller (controller)
  ((esum :accessor pid-error-sum :initform 0.0)
   (perr :accessor pid-previous-error :initform 0.0)
   (last-update :reader pid-last-update :initform 0.0)
   (kp :accessor pid-gain :initarg :gain)
   (ki :accessor pid-reset :initarg :reset)
   (kd :accessor pid-derivative :initarg :derivative)
   (sp :accessor pid-setpoint :initarg :setpoint)))

(defgeneric error-function (controller pv time))
(defgeneric get-process-value (control-type spatial time))
(defgeneric update-manipulated-value (control-type spatial app-time new-mv))

(defmethod error-function ((pid pid-controller) pv time)
  (with-slots (esum perr last-update sp) pid
    (let* ((dt (- time last-update))
	   (err (- pv sp)))
      (setf perr esum)
      (incf esum (* err dt))
      (setf last-update time)
      err)))

(defmethod update-controllers ((pid pid-controller) spatial dt app-time)
  (let ((err (error-function pid (get-process-value pid spatial app-time) app-time)))
    (with-slots (kp ki kd esum perr) pid
      (let* ((derr (- (e perr)))
	     (mv (+ (* kp err)
		    (* (/ 1 ki) esum)
		    (* kd derr))))
	(update-manipulated-value pid spatial app-time mv)))))
    
    
  